/**********************************************************************\
*																																	    *
* PROGRAM: Common file for Get Over It 2010-2011  	      					  *
* VERSION: 2.0																										    *
* PURPOSE: Common file holding all common procedures                  *
* AUTHOR: Team R.A.B.B.I.                                             *
* DATE:		 April 2011  	  																					  *
*																																		  *
* LICENSE: GNU GPL V3 2011                                            *
\**********************************************************************/


/*
* Movements for Omni-bot according to above diagram
* @pragma speed: speed for the robot to travel from 0-100
* @void drive: @pragma percent: percent speed for all movements. From 0-100
*/

#pragma systemFile
#ifndef __RABBI_COMMON__
#define __RABBI_COMMON__
#endif

#ifndef OMNIDRIVER
#include "OMNI-driver.c"
#endif

bool _isBetween(int x, int threshold, int plusMinus) {
  if (clip(x, threshold - plusMinus, threshold + plusMinus) == x) {
    return true;
  }
  return false;
}


void stopmot() {
  motor[a]=0;
  motor[b]=0;
  motor[c]=0;
  motor[d]=0;
  motor[e]=0;
  motor[f]=0;
  wait10Msec(10);
  nMotorEncoder[b] = 0;
  nMotorEncoder[c] = 0;
}
void left(int speed){
  motor[a]=speed;
  motor[b]=-speed;
  motor[c]=speed ;
  motor[d]=-speed;
}
void right(int speed){
  motor[a]= -speed;
  motor[b]= speed;
  motor[c]= -speed;
  motor[d]= speed;
}
void forward(int speed){
  motor[a]=speed;
  motor[b]=speed;
  motor[c]=speed;
  motor[d]=speed;}
void backward (int speed){
  motor[a]=-speed;
  motor[b]=-speed;
  motor[c]=-speed;
  motor[d]=-speed;
}
void clk(int speed) {
  motor[a] = speed;
  motor[b] = speed;
  motor[c] = -speed;
  motor[d] = -speed;
}

void cntClk(int speed) {
  motor[a] = -speed;
  motor[b] = -speed;
  motor[c] = speed;
  motor[d] = speed;
}


void frontl(int speed){ // slant forward to the left
  int slower = (speed>0) ? (speed/SLOWER_SPD) : (-speed/SLOWER_SPD);
  motor[a]=speed;
  motor[b]=-slower;
  motor[c]=speed;
  motor[d]=slower;
}

void frontr(int speed){                        // slant forward to the right
  int slower = (speed>0) ? (speed/SLOWER_SPD) : (-speed/SLOWER_SPD);
  motor[a]=slower;
  motor[b]=speed;
  motor[c]=-slower;
  motor[d]=speed;
}



void drive(int percent) {
  motor[a] = ((float)Jdrivea)* (((float)percent)/100);
  motor[b] = ((float)Jdriveb)* (((float)percent)/100);
  motor[c] = ((float)Jdrivec)* (((float)percent)/100);
  motor[d] = ((float)Jdrived)* (((float)percent)/100);
}
